#include "GWMatrix.h"
#include <cstring>
#include <math.h>

const Matrix4 Matrix4::IDENTITY =
	Matrix4(1.f, 0.f, 0.f, 0.f,
			0.f, 1.f, 0.f, 0.f,
			0.f, 0.f, 1.f, 0.f,
			0.f, 0.f, 0.f, 1.f);

const Matrix4& Matrix4::operator=( const Matrix4& rhs )
{
	memcpy( this, &rhs.m11, sizeof(Matrix4) );
	return *this;
}

Matrix4 Matrix4::operator*( const Matrix4& rhs ) const
{
	Matrix4 r;

	r.m11 = m11 * rhs.m11 + m12 * rhs.m21 + m13 * rhs.m31 + m14 * rhs.m41;
	r.m12 = m11 * rhs.m12 + m12 * rhs.m22 + m13 * rhs.m32 + m14 * rhs.m42;
	r.m13 = m11 * rhs.m13 + m12 * rhs.m23 + m13 * rhs.m33 + m14 * rhs.m43;
	r.m14 = m11 * rhs.m14 + m12 * rhs.m24 + m13 * rhs.m34 + m14 * rhs.m44;

	r.m21 = m21 * rhs.m11 + m22 * rhs.m21 + m23 * rhs.m31 + m24 * rhs.m41;
	r.m22 = m21 * rhs.m12 + m22 * rhs.m22 + m23 * rhs.m32 + m24 * rhs.m42;
	r.m23 = m21 * rhs.m13 + m22 * rhs.m23 + m23 * rhs.m33 + m24 * rhs.m43;
	r.m24 = m21 * rhs.m14 + m22 * rhs.m24 + m23 * rhs.m34 + m24 * rhs.m44;

	r.m31 = m31 * rhs.m11 + m32 * rhs.m21 + m33 * rhs.m31 + m34 * rhs.m41;
	r.m32 = m31 * rhs.m12 + m32 * rhs.m22 + m33 * rhs.m32 + m34 * rhs.m42;
	r.m33 = m31 * rhs.m13 + m32 * rhs.m23 + m33 * rhs.m33 + m34 * rhs.m43;
	r.m34 = m31 * rhs.m14 + m32 * rhs.m24 + m33 * rhs.m34 + m34 * rhs.m44;

	r.m41 = m41 * rhs.m11 + m42 * rhs.m21 + m43 * rhs.m31 + m44 * rhs.m41;
	r.m42 = m41 * rhs.m12 + m42 * rhs.m22 + m43 * rhs.m32 + m44 * rhs.m42;
	r.m43 = m41 * rhs.m13 + m42 * rhs.m23 + m43 * rhs.m33 + m44 * rhs.m43;
	r.m44 = m41 * rhs.m14 + m42 * rhs.m24 + m43 * rhs.m34 + m44 * rhs.m44;

	return r;
}

Matrix4 Matrix4::BuildRotationMatrix( float rad, ROTATION_AXIS axis )
{
	switch ( axis)
	{
	case ROTATION_AXIS_X:
		return Matrix4( 1.f, 0.f, 0.f, 0.f,
						0.f, cosf(rad),	-sinf(rad), 0.f,
						0.f, sinf(rad), cosf(rad), 0.f,
						0.f, 0.f, 0.f, 1.f );
	case ROTATION_AXIS_Y:
		return Matrix4( cosf(rad), 0.f, sinf(rad), 0.f,
						0.f, 1.f, 0.f, 0.f,
						-sinf(rad), 0.f, cosf(rad), 0.f,
						0.f, 0.f, 0.f, 1.f );
	case ROTATION_AXIS_Z:
		return Matrix4( cosf(rad), -sinf(rad), 0.f, 0.f,
						sinf(rad), cosf(rad), 0.f, 0.f,
						0.f, 0.f, 1.f, 0.f,
						0.f, 0.f, 0.f, 1.f );
	}

	return Matrix4::IDENTITY;
}

Matrix4 Matrix4::BuildTranslationMatrix( const Vector3& trans )
{
	return Matrix4( 1.f, 0.f, 0.f, 0.f,
					0.f, 1.f, 0.f, 0.f,
					0.f, 0.f, 1.f, 0.f,
					trans.x, trans.y, trans.z, 1.f );
}
